#include<ros/ros.h>
#include<geometry_msgs/PoseStamped.h>
#include<tf/transform_listener.h>
#include<iostream>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "publish_target_pose0");
    if(argc!=3)
    {
        ROS_INFO("请输入tf树的父节点和子节点");
        return -1;
    }
    std::string parent_link,child_link;
    parent_link=argv[1];
    child_link=argv[2];
    geometry_msgs::PoseStamped ps;
    ps.header.frame_id="frame_target";
    ros::NodeHandle nh;
    ros::Publisher target_publisher=nh.advertise<geometry_msgs::PoseStamped>("target_pose",5);
    tf::TransformListener tfListener;
    tf::StampedTransform transform;
    while(1)
    {
        std::cout<<"请输入字符 a 开始启动机械臂。\n";
        char signCheck;
        std::cin>>signCheck;
        if(signCheck=='a')
        {
            std::cout<<"开始运动到指定位置。\n";
            tfListener.lookupTransform(parent_link,child_link,ros::Time(0),transform);
            auto position=transform.getOrigin();
            auto rotation=transform.getRotation();
            ps.pose.position.x=position.x();
            ps.pose.position.y=position.y();
            ps.pose.position.z=position.z();
            ps.pose.orientation.w=rotation.getW();
            ps.pose.orientation.x=rotation.getX();
            ps.pose.orientation.y=rotation.getY();
            ps.pose.orientation.z=rotation.getZ();
            target_publisher.publish(ps);
        }



    }





}
